#ifndef __H_GRAPH_H__
#define __H_GRAPH_H__

#include "Graph.h"
#include "CollisionDetector.h"
#include <vector>
#include <list>

namespace {
  class LessThenPointD {
    public:
      bool operator()(const Point_d& p1, const Point_d& p2) const {
        for (int i = 0; i < 4; ++i) {
          if (p1[i] == p2[i]) {
            continue;
          }
          return p1[i] < p2[i];
        }
        return false;
      }
  };
}  // namespace

class HGraph {
  public:
    HGraph(const CollisionDetector& collisionDetection);
    void addPathList(const std::vector<Point_d>& pathList);

    std::list<Point_d> findPath();

  private:
    Graph<Point_d, LessThenPointD> m_graph;
    vector<vector<Point_d> > m_pathsLists;
    const CollisionDetector* m_collisionDetection;
};

#endif  // __H_GRAPH_H__
